
import { Bone, Euler, Group, Matrix4, Quaternion, Vector3 } from "three";


type BoneName = string
type name = string


export class Joint {



    boneInitDirection: Vector3[] = []


    constructor(public root: Group, public nameMap: Array<[BoneName, name]>, public initQuaternion: Quaternion = new Quaternion()) {


        const boneWorldPositions = nameMap.map(([boneName, _]) => {
            const target = root.getObjectByName(boneName);
            const v3 = new Vector3();
            target!.getWorldPosition(v3);
            return v3
        })

        this.boneInitDirection = this.computeBoneDirection(boneWorldPositions)


    }


    update(states: Record<string, { position: Vector3, wight: number }>) {



        const worldPositions = this.nameMap.map(([_, name]) => states[name].position)

        const localVectors = this.computeBoneDirection(worldPositions)




        this.nameMap.slice(0, -1).forEach((_, index) => {
            const qu = new Quaternion();
            if (index >= 1) {
                const v3 = this.boneInitDirection[index - 1].clone();
                qu.setFromUnitVectors(new Vector3(0, 0, 1), v3.clone().normalize());

                const v32 = this.boneInitDirection[index].clone();

                v32.applyQuaternion(qu.clone());

                const qu2 = new Quaternion();

                qu2.setFromUnitVectors(v32, localVectors[index]);

                qu.multiply(qu2.clone());
            } else {


                qu.setFromUnitVectors(this.boneInitDirection[index].clone(), localVectors[index]);

                qu.premultiply(this.initQuaternion.clone());
                // qu.multiply(this.initQuaternion.clone().invert());
                // qu = this.initQuaternion.clone().multiply(qu);
            }

            const target = this.root.getObjectByName(this.nameMap[index][0]) as Bone;

            if (target) {
                target.quaternion.copy(qu);

                // target.weight

            }

        })



    }




    computeBoneDirection(worldPositions: Vector3[]) {
        const localVectors: Vector3[] = []




        const matrix = new Matrix4()
        matrix.makeTranslation(worldPositions[0].x, worldPositions[0].y, worldPositions[0].z)
        const mt4 = new Matrix4()
        const mt4_2 = new Matrix4()
        const quad = new Quaternion()

        // 处理每个子节点
        for (let i = 1; i < worldPositions.length; i++) {

            // const gradeParent = worldPositions[i - 2]
            const parentPos = worldPositions[i - 1].clone()
            const nodeWorldPos = worldPositions[i].clone();


            const v3 = nodeWorldPos.applyMatrix4(matrix.clone().invert());

            const v3Normal = v3.clone().normalize()

            localVectors.push(v3Normal)



            mt4.makeTranslation(v3)


            if (parentPos) {

                const base = new Vector3()
                base.setFromMatrixColumn(matrix, 2).normalize();

                quad.setFromUnitVectors(base, v3Normal)

                mt4_2.makeRotationFromQuaternion(quad)

                mt4.multiply(mt4_2)
            }

            matrix.multiply(mt4)
        }

        return localVectors



    }
}